/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package subsystems;

import commands.DriveWithJoystick;
import driver.SPI;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.command.Subsystem;
import framework.HW;

/**
 *
 * @author David
 */
public class DriveBase extends Subsystem {
    
     private CANJaguar jag_1,jag_2,jag_3,jag_4;
     private CANJaguar[] jag_arr;
     private Encoder left_encoder;
     private Encoder right_encoder;
     private SPI spi_imu;
    
    public void initDefaultCommand(){
        setDefaultCommand(new DriveWithJoystick());
    }
    
  
    
    public void setLeft(double left_speed){
        try{
            jag_1.setX(left_speed);
            jag_3.setX(left_speed);
        }catch(Exception e){e.getMessage();}
    }
    
    public void setRight(double right_speed){
        try {
            jag_2.setX(-1*right_speed);
            jag_4.setX(-1*right_speed);
        } catch (CANTimeoutException ex) {
            System.out.println(ex.getMessage());
        }
       
        
    }
    
    public SPI getSPI(){
        return spi_imu;
    }
    
    public Encoder getLeftEncoder(){
        return left_encoder;
    }
    
    public Encoder getRightEncoder(){
        return right_encoder;
    }
    
    /**
     * START INIT COMMANDS
     */
    public void defEncoders(){
        left_encoder = new Encoder(HW.leftEncoder_spg,HW.leftEncoder_s);
        right_encoder = new Encoder(HW.rightEncoder_spg,HW.rightEncoder_s);
    }
        
    public void defSPI(){
        spi_imu = new SPI(HW.spiSlot, HW.spiSCK, HW.spiMOSI, HW.spiMISO, HW.spiSELC);
    }
    
    public void setJaguars(){
        jag_arr = new CANJaguar[4];
        jag_1 = defJaguar(HW.motor_1);
        jag_arr[0] = jag_1;
        jag_2 = defJaguar(HW.motor_2);
        jag_arr[1] = jag_2;
        jag_3 = defJaguar(HW.motor_3);
        jag_arr[2] = jag_3;
        jag_4 = defJaguar(HW.motor_4);
        jag_arr[3] = jag_4;
    }
    
    /**
     * END INIT COMMANDS
     */
    
    public CANJaguar getJaguar(int id){
        return jag_arr[id-1];
    }
    
    public CANJaguar[] getJagArr(){
        return jag_arr;
    }
    
    
    protected CANJaguar defJaguar(int id){
        
        CANJaguar jag = null;
        try{
       // System.out.println("Trying to connect to CAN port " + id + "...");
         System.out.println("connecting...");
        jag = new CANJaguar(id);
        }catch(Exception e){System.out.println(e.getMessage());}
       // System.out.println("Connected to CAN port " + id);
        return jag;
    }

    
   
   
}
